////////////////////////
// L为下三角矩阵，Ly=b
// n为矩阵的秩
// 解得的y储存在b中
////////////////////////
int Lower_Triangular_Matrix(double** L, double* b, int n);
////////////////////////
// L为单位下三角矩阵，Ly=b
// n为矩阵的秩
// 解得的y储存在b中
////////////////////////
int Unit_Lower_Triangular_Matrix(double** L, double* b, int n);
////////////////////////
// U为上三角矩阵，Ux=y
// n为矩阵的秩
// 解得的x储存在y中
////////////////////////
int Upper_Triangular_Matrix(double** U, double* y, int n);
////////////////////////
// U为单位上三角矩阵，Ux=y
// n为矩阵的秩
// 解得的x储存在y中
////////////////////////
int Unit_Upper_Triangular_Matrix(double** U, double* y, int n);
////////////////////
// A为n×n矩阵，求A的三角分解
// A=LU，其中L为单位下三角阵，U为上三角阵
// 结果储存在A的内存中，A的上三角部分为U，其余部分为L除主对角线外的元素
////////////////////
int Gaussian_Elimination(double** A, int n);
//////////////////////////////
// 列主元高斯消去法Ax=b
// A为n×n矩阵，求A的三角分解
// PA=LU，其中L为单位下三角阵，U为上三角阵
// 结果储存在A的内存中，A的上三角部分为U，其余部分为L除主对角线外的元素
//////////////////////////////
int Column_Principle_Gaussian_Elimination(double** A, double* b, int n);
/////////////////////////////
//平方根法LL(T)分解
//求解Ax=b
//其中A是对称正定矩阵
//L为下三角矩阵，也称为A的Cholesky因子
//计算所得的L储存在A的对应位置上
/////////////////////////////
int Root_Squaring(double** A, int n);
/////////////////////////////
//改进的平方根法LDL(T)分解
//求解Ax=b
//其中A是对称正定矩阵
//L为单位下三角矩阵，D为对角阵
//计算所得的L除主对角线储存在A的对应位置上
//D储存在A的主对角线上
/////////////////////////////
int Root_Squaring_pro(double** A, int n);
//////////////////////////////
//求解Dx=y
//其中D为非奇异对角阵
//计算的结果储存在y中
//////////////////////////////
int Diag_Matrix(double** D, double* y, int n);
///////////////////////////////
//求矩阵A的转置并返回转置后矩阵的地址
//A的转置结果储存在B中，没有修改A的矩阵值
///////////////////////////////
void Transposition(double** A, double** B, int n);
///////////////////////////////
//符号函数
//结果储存在v中，没有修改原来的矩阵值
///////////////////////////////
int signature(double* w, double* v, int n);
///////////////////////////////
//返回R^n向量的无穷范数
///////////////////////////////
double Norms_Vector_Infinite(double* x, int n);
///////////////////////////////
//返回R^n向量的1范数
///////////////////////////////
double Norms_Vector_One(double* x, int n);
///////////////////////////////
//返回两个R^n向量的乘积
///////////////////////////////
double Vector_Product(double* x, double* y, int n);
///////////////////////////////
//求两个R^n向量的差
//差储存在y中
///////////////////////////////
int Vector_Difference(double* x, double* y, int n);
///////////////////////////////
//求两个R^n向量的差
//差储存在z中
///////////////////////////////
void Vector_Difference(double* x, double* y, double* z, int n);
///////////////////////////////
//求n×n矩阵A和一个向量x的乘积
//所得结果储存在y中
///////////////////////////////
int Matrix_Vector_Product(double** A, double* x, double* y, int n);
///////////////////////////////
//估计矩阵的1范数时对x做更新操作
//x = ej，其中向量z的无穷范数在第j个元素取到
///////////////////////////////
int renew(double* x, double* z, int n);
///////////////////////////////
//估计矩阵A的逆的无穷范数
//返回估计值
///////////////////////////////
double Norms_Inverse_Matrix_Infinite(double** A, int n);
/////////////////////////
//返回矩阵A的无穷范数
//即行和范数
/////////////////////////
double Norms_Matrix_Infinite(double** A, int n);
//////////////////////////
//求矩阵A无穷范数的条件数
//////////////////////////
double Condition_Number_Norm_Infinite(double** A, int n);
//////////////////////////
//返回计算解cal_x和真实解real_x的相对误差的估计
//Ax=b，其中b=A×real_x
//////////////////////////
double Precision_by_estimate(double** A, double* _b, double* cal_x, double* real_x, int n);
///////////////////////////////
//返回计算解cal_x和真实解real_x的真实相对误差
///////////////////////////////
double Precision_real(double* cal_x, double* real_x, int n);
//////////////////////////////////////////
//同时输出精确解和数值解的相对误差估计和相对误差
//Ax=_b,cal_x为数值解，real_x为精确解
/////////////////////////////////////////
void Precision(double** A, double* _b, double* cal_x, double* real_x, int n);
////////////////////////////////
//返回向量x的2范数
////////////////////////////////
double Norm_Vector_2(double* x, int n);
////////////////////////////////
//Jacobi迭代法
////////////////////////////////
void Jacobi_Iteration(double** A, double* b, double* x,  double condition, int n);
//////////////////////////////
//Gauss-Seidel迭代
//////////////////////////////
void GS_Iteration(double** A, double* b, double* x, double condition, int n);
//////////////////////////////////////
//SOR迭代
/////////////////////////////////////
void SOR(double** A, double* b, double* x, double w, double condition, int n);
//////////////////////////////////////
//共轭梯度法
//////////////////////////////////////
void ConjugateGradient(double** A, double* b, double* x, int n);
///////////////////////////////
//求两个R^n向量的和
//差储存在z中
///////////////////////////////
void Vector_Add(double* x, double* y, double* z, int n);
/////////////////////////////////
//幂法求按模最大特征值
/////////////////////////////////
void EigenvaluePower(double** A,double condition, int n);
///////////////////////////////////////////////
#include <iostream>
#include <math.h>



////////////////////////
// L为下三角矩阵，Ly=b
// n为矩阵的秩
// 解得的y储存在b中
////////////////////////
int Lower_Triangular_Matrix(double** L, double* b, int n)
{
  for(int j = 0; j < n-1 ; j++)
    {
      b[j] = b[j]/L[j][j];
      for(int i = j + 1; i < n ; i++)
	{
	  b[i] = b[i]-b[j]*L[i][j];
	}
    }
  b[n-1] = b[n-1]/L[n-1][n-1];
  return 0;
};

////////////////////////
// L为单位下三角矩阵，Ly=b
// n为矩阵的秩
// 解得的y储存在b中
////////////////////////
int Unit_Lower_Triangular_Matrix(double** L, double* b, int n)
{
  for(int j = 0; j < n-1 ; j++)
    {
      for(int i = j + 1; i < n ; i++)
	{
	  b[i] = b[i]-b[j]*L[i][j];
	}
    }
  return 0;
};



////////////////////////
// U为上三角矩阵，Ux=y
// n为矩阵的秩
// 解得的x储存在y中
////////////////////////
int Upper_Triangular_Matrix(double** U, double* y, int n)
{
  for(int j = n-1; j > 0 ; j--)
    {
      y[j] = y[j]/U[j][j];
      for(int i = 0; i < j ; i++)
	{
	  y[i] = y[i]-y[j]*U[i][j];
	}
    }
  y[0] = y[0]/U[0][0];
  return 0;
};

////////////////////////
// U为单位上三角矩阵，Ux=y
// n为矩阵的秩
// 解得的x储存在y中
////////////////////////
int Unit_Upper_Triangular_Matrix(double** U, double* y, int n)
{
  for(int j = n-1; j > 0 ; j--)
    {
      y[j] = y[j];
      for(int i = 0; i < j ; i++)
	{
	  y[i] = y[i]-y[j]*U[i][j];
	}
    }
  return 0;
};



////////////////////
// A为n×n矩阵，求A的三角分解
// A=LU，其中L为单位下三角阵，U为上三角阵
// 结果储存在A的内存中，A的上三角部分为U，其余部分为L除主对角线外的元素
////////////////////
int Gaussian_Elimination(double** A, int n)
{
  for(int k = 0; k < n - 1; k++)
    {
      for(int i = k + 1; i < n; i++)
	{
	  A[i][k] = A[i][k]/A[k][k];
	}
      for(int i = k + 1; i < n; i++)
	{
	  for(int j = k + 1; j < n; j++)
	    {
	      A[i][j] = A[i][j] - A[i][k]*A[k][j];
	    }
	}
    }
  return 0;
};

//////////////////////////////
// 列主元高斯消去法
// A为n×n矩阵，求A的三角分解
// PA=LU，其中L为单位下三角阵，U为上三角阵
// 结果储存在A的内存中，A的上三角部分为U，其余部分为L除主对角线外的元素
//////////////////////////////
int Column_Principle_Gaussian_Elimination(double** A, double* b, int n)
{
  for(int k = 0; k < n - 1; k++)
    {
      int p = k;   //p为第k列中最大的元素行数
      for(int i = k + 1; i < n; i++)
	{
	  if(fabs(A[i][k]) > fabs(A[p][k]))
	    p = i;
	}
      if(p != k)   //对A进行初等变换，把k、p行互换
	{
	  for(int i = 0; i < n; i++)
	    {
	      double tmp = A[p][i];
	      A[p][i] = A[k][i];
	      A[k][i] = tmp;
	    }
	  double tmp = b[p];
	  b[p] = b[k];
	  b[k] = tmp;
	}
      
      if(A[k][k] != 0)
	{
	  for(int i = k + 1; i < n; i++)
	    {
	      A[i][k] = A[i][k]/A[k][k];
	    }
	  for(int i = k + 1; i < n; i++)
	    {
	      for(int j = k + 1; j < n; j++)
		{
		  A[i][j] = A[i][j] - A[i][k]*A[k][j];
		}
	    }
	}
      else
	{
	  std::cout << "Error! A is a singular matrix." << std::endl;
	  return -1;
	} 
    }

  return 0;
};

//////////////////////////////
// 列主元高斯消去法
// A为n×n矩阵，求A的三角分解
// PA=LU，其中L为单位下三角阵，U为上三角阵
// 结果储存在A的内存中，A的上三角部分为U，其余部分为L除主对角线外的元素
// b中储存A行变换的信息
//////////////////////////////
int Column_Principle_Gaussian_Elimination(double** A, int* b, int n)
{
  for(int k = 0; k < n - 1; k++)
    {
      int p = k;   //p为第k列中最大的元素行数
      for(int i = k + 1; i < n; i++)
	{
	  if(fabs(A[i][k]) > fabs(A[p][k]))
	    p = i;
	}
      b[k] = p;
      if(p != k)   //对A进行初等变换，把k、p行互换
	{
	  for(int i = 0; i < n; i++)
	    {
	      double tmp = A[p][i];
	      A[p][i] = A[k][i];
	      A[k][i] = tmp;
	    }
	}
      
      if(A[k][k] != 0)
	{
	  for(int i = k + 1; i < n; i++)
	    {
	      A[i][k] = A[i][k]/A[k][k];
	    }
	  for(int i = k + 1; i < n; i++)
	    {
	      for(int j = k + 1; j < n; j++)
		{
		  A[i][j] = A[i][j] - A[i][k]*A[k][j];
		}
	    }
	}
      else
	{
	  std::cout << "Error! A is a singular matrix." << std::endl;
	  return -1;
	} 
    }

  return 0;
};

/////////////////////////////
//平方根法LL(T)分解
//求解Ax=b
//其中A是对称正定矩阵
//L为下三角矩阵，也称为A的Cholesky因子
//计算所得的L储存在A的对应位置上
/////////////////////////////
int Root_Squaring(double** A, int n)
{
  for(int k = 0; k < n; k++)
    {
      if(A[k][k] >= 0)
	{
	  A[k][k] = sqrt(A[k][k]);
	  for(int i = k + 1; i < n; i++)
	    {
	      A[i][k] = A[i][k] / A[k][k];
	    }
	  for(int i = k + 1; i < n; i++)
	    {
	      for(int j = i; j < n; j++)
		A[j][i] = A[j][i] - A[j][k]*A[i][k];
	    }
	  //将A的上三角部分填充为下三角的倒置
	  for(int i = 0; i < n; i++)
	    {
	      for(int j = i+1; j < n; j++)
		A[i][j] = A[j][i];
	    }
	  return 0;
	}
      else
	{
	  std::cout << "Eorror! Can't use root-squaring method!" << std::endl;
	  return -1;
	}
    }
    return 0;
};

/////////////////////////////
//改进的平方根法LDL(T)分解
//求解Ax=b
//其中A是对称正定矩阵
//L为单位下三角矩阵，D为对角阵
//计算所得的L除主对角线储存在A的对应位置上
//D储存在A的主对角线上
/////////////////////////////
int Root_Squaring_pro(double** A, int n)
{
  double* v = new double[n]();
  for(int j = 0; j < n; j++)
    {
      for(int i = 0; i < j-1; i++)
	v[i] = A[j][i]*A[i][i];
      for(int i = 0; i < j-1; i++)
	A[j][j] = A[j][j] - A[j][i]*v[i];
      for(int i = j + 1; i < n; i++)
	{
	  for(int k = 0; k < j-1; k++)
	    A[i][j] = A[i][j] - A[i][k]*v[k];
	  A[i][j] = A[i][j]/A[j][j];
	}
    }
  //将A的上三角部分填充为下三角的倒置
  for(int i = 0; i < n; i++)
    {
      for(int j = i+1; j < n; j++)
       	A[i][j] = A[j][i];
    }
  
  delete [] v;

  return 0;
};

//////////////////////////////
//求解Dx=y
//其中D为非奇异对角阵
//计算的结果储存在y中
//////////////////////////////
int Diag_Matrix(double** D, double* y, int n)
{
  for(int i = 0; i < n; i++)
    {
      if(D[i][i] != 0)
	y[i] = y[i]/D[i][i];
      else
	{
	  std::cout << "Error!" << std::endl;
	  return -1;
	}
    }
  
  return 0;
};

///////////////////////////////
//求矩阵A的转置并返回转置后矩阵的地址
//A的转置结果储存在B中，没有修改A的矩阵值
///////////////////////////////
void Transposition(double** A, double** B, int n)
{
  for(int i = 0; i < n; i++)
    {
      for(int j = 0; j < n; j++)
	B[i][j] = A[j][i];
    }
};

///////////////////////////////
//符号函数
//结果储存在v中，没有修改原来的矩阵值
///////////////////////////////
int signature(double* w, double* v, int n)
{
  for(int i = 0; i < n; i++)
    {
      if (w[i] < 0)
	v[i] = -1.0;
      else if(w[i] == 0)
	v[i] = 0.0;
      else
	v[i] = 1.0;
    }
  return 0;
};

///////////////////////////////
//返回R^n向量的无穷范数
///////////////////////////////
double Norms_Vector_Infinite(double* x, int n)
{
  double norm = 0.0;
  for(int i = 0; i < n; i++)
    {
      if (fabs(x[i]) > norm)
	norm = fabs(x[i]);
    }
  return norm;
};

///////////////////////////////
//返回R^n向量的1范数
///////////////////////////////
double Norms_Vector_One(double* x, int n)
{
  double norm = 0.0;
  for(int i = 0; i < n; i++)
    {
      norm += fabs(x[i]);
    }
  return norm;
};

///////////////////////////////
//返回两个R^n向量的乘积
///////////////////////////////
double Vector_Product(double* x, double* y, int n)
{
  double product = 0.0;
  for(int i = 0; i < n; i++)
    product += x[i]*y[i];
  return product;
};

///////////////////////////////
//求两个R^n向量的差
//差储存在y中
///////////////////////////////
int Vector_Difference(double* x, double* y, int n)
{
  for(int i = 0; i < n; i++)
    y[i] = x[i] - y[i];
  return 0;
};

///////////////////////////////
//求n×n矩阵A和一个向量x的乘积
//所得结果储存在y中
///////////////////////////////
int Matrix_Vector_Product(double** A, double* x, double* y, int n)
{
  for(int i = 0; i < n; i++)
    {
      y[i] = 0.0;
      for(int j = 0; j < n; j++)
	y[i] += A[i][j]*x[j];
    }
  return 0;
};

///////////////////////////////
//估计矩阵的1范数时对x做更新操作
//x = ej，其中向量z的无穷范数在第j个元素取到
///////////////////////////////
int renew(double* x, double* z, int n)
{
  int j = 0;
  double norm = 0.0;
  for(int i = 0; i < n; i++)
    {
      if (fabs(z[i]) > norm)
	{
	  norm = fabs(z[i]);
	  j = i;
	}	
    }
  for(int i = 0; i < n; i++)
    x[i] = 0.0;
  x[j] = 1.0;
  return 0;
};

///////////////////////////////
//估计矩阵A的逆的无穷范数
//返回估计值
///////////////////////////////
double Norms_Inverse_Matrix_Infinite(double** A, int n)
{
  int k = 1;
  double norm_one = 0.0;
  double** B = new double*[n];
  double** C = new double*[n];
  for(int i = 0; i < n; i++)
    {
      B[i] = new double[n]();
      C[i] = new double[n]();
      for(int j = 0; j < n; j++)
	C[i][j] = A[i][j];
    } 
  Transposition(C, B, n);
  //B为A的转置
  double* y = new double[n]();
  double* x = new double[n]();
  for(int i = 0; i < n; i++)
    y[i] = x[i] = 1.0 / n;
  //y中记录x
  int* b = new int[n]();
  Column_Principle_Gaussian_Elimination(B, b, n);
  int* p = new int[n]();
  Column_Principle_Gaussian_Elimination(C, p, n);
  //对A、B进行列主元分解
  double* v = new double[n]();
  while( k == 1 )
    { 
      for(int i = 0; i < n; i++)
	{
	  if(b[i] > i)
	    {
	      double tmp = y[i];
	      y[i] = y[b[i]];
	      y[b[i]] = tmp;
	    }
	}
       Unit_Lower_Triangular_Matrix(B, y, n);
       Upper_Triangular_Matrix(B, y, n);
       //解Bw=y，y中储存w
       signature(y, v, n);
       //v为w的符号向量
       for(int i = 0; i < n; i++)
	{
	  if(p[i] > i)
	    {
	      double tmp = v[i];
	      v[i] = v[p[i]];
	      v[p[i]] = tmp;
	    }
	}
       Unit_Lower_Triangular_Matrix(C, v, n);
       Upper_Triangular_Matrix(C, v, n);
       //解Az=v，v中储存z
       if(Norms_Vector_Infinite(v, n) <= Vector_Product(v, x, n))
	 {
	   norm_one = Norms_Vector_One(y, n);
	   k = 0;
	   for(int i = 0; i < n; i++)
	     {
	       delete [] C[i];
	       delete [] B[i];
	     }
	   delete [] B;
	   delete [] C;
	   delete [] y;
	   delete [] x;
	   delete [] v;
	 }
       else
	 {
	   renew(x, v, n);
	   renew(y, v, n);
	   k = 1;
	 }
       
    }
  return norm_one;
};

/////////////////////////
//返回矩阵A的无穷范数
//即行和范数
/////////////////////////
double Norms_Matrix_Infinite(double** A, int n)
{
  double norm_infinite = 0.0;
  for(int i = 0; i < n; i++)
    {
      double sum = 0.0;
      for(int j = 0; j < n; j++)
	{
	  sum += fabs(A[i][j]);
	}
      if(sum > norm_infinite)
	norm_infinite = sum;
    }
  return norm_infinite;
};

//////////////////////////
//求矩阵A无穷范数的条件数
//////////////////////////
double Condition_Number_Norm_Infinite(double** A, int n)
{
  double norm_infinite = Norms_Matrix_Infinite(A,n);
  double norm_inverse_infinite = Norms_Inverse_Matrix_Infinite(A,n);
  return norm_infinite*norm_inverse_infinite;
};

//////////////////////////
//返回计算解cal_x和真实解real_x的相对误差的估计
//Ax=b，其中b=A×real_x
//////////////////////////
double Precision_by_estimate(double** A, double* _b, double* cal_x, int n)
{
  double v = Condition_Number_Norm_Infinite(A, n);
  double* x = new double[n]();
  Matrix_Vector_Product(A, cal_x, x, n);
  for(int i = 0; i < n; i++)
    {
      x[i] = x[i] - _b[i];
    }
  double r = Norms_Vector_Infinite(x, n);
  double b = Norms_Vector_Infinite(_b, n);
  delete [] x;
  return v*r/b;
};

///////////////////////////////
//返回计算解cal_x和真实解real_x的真实相对误差
///////////////////////////////
double Precision_real(double* cal_x, double* real_x, int n)
{
  double* x = new double[n]();
  for(int i = 0; i < n; i++)
    {
      x[i] = cal_x[i] - real_x[i];
    }
  double r = Norms_Vector_Infinite(x, n);
  double R = Norms_Vector_Infinite(real_x, n);
  delete [] x;
  return r/R;
};

//////////////////////////////////////////
//同时输出精确解和数值解的相对误差估计和相对误差
//Ax=_b,cal_x为数值解，real_x为精确解
/////////////////////////////////////////
void Precision(double** A, double* _b, double* cal_x, double* real_x, int n)
{
  std::cout << "The precision by estimate of " << n << "-order matrix is " <<
    Precision_by_estimate(A, _b, cal_x, n) << "." << std::endl;
  std::cout << "The real precision of " << n << "-order matrix is " <<
    Precision_real(cal_x, real_x, n) << "." << std::endl;
};

//////////////////////////////////////////
//向量R^n数乘a
//////////////////////////////////////////
void num_vector_product(double* x, double a, int n)
{
  for(int i = 0; i < n; i++)
    {
      x[i] = x[i]*a;
    }
};

//////////////////////////////////////////
//矩阵R^n×n数乘a
//////////////////////////////////////////
void num_matrix_product(double** A, double a, int n)
{
  for(int i = 0; i < n; i++)
    {
      for(int j = 0; j < n; j++)
	A[i][j] = A[i][j]*a;
    }
};

///////////////////////////////////////////
//Householder变换
//k表示矩阵A的第k列，m表示矩阵的行数,n表示矩阵列数(m<=n)
//对A中第k列下方长度为m-k的向量求householder变换
//求得v为第一个元素为1，长度为m-k的向量,储存在x中
//v[1:m-k]储存在A[k+1:m][k]中
///////////////////////////////////////////
double Householder(double** A, double* x, int k, int m)
{
  int l = m-k; //向量x的长度
  for(int i = 0; i < l; i++)
    x[i] = A[k+i][k];
  num_vector_product(x, 1.0/Norms_Vector_Infinite(x, l), l);
  double p = 0.0; //向量x除去第一个元素的平方和
  double b = 0.0;
  for(int i = 1; i < l ; i++)
    {
      p += x[i]*x[i];
    }
  if(p == 0.0)
    b = 0.0;
  else
    {
      double a = pow(x[0]*x[0]+p, 0.5); //a为x的2范数
      if( x[0] <= 0)
	x[0] -= a;
      else
	x[0] = (-1)*p/(x[0]+a);
      b = 2*x[0]*x[0]/(p+x[0]*x[0]);
      num_vector_product(x, 1.0/x[0], l);
    }

  return b;
};

//////////////////////////////////////
//求长度为l的向量y的Householder变换
/////////////////////////////////////
double Householder(double* y, double* x, int l)
{
  for(int i = 0; i < l; i++)
    x[i] = y[i];
  num_vector_product(x, 1.0/Norms_Vector_Infinite(x, l), l);
  double p = 0.0; //向量x除去第一个元素的平方和
  double b = 0.0;
  for(int i = 1; i < l ; i++)
    {
      p += x[i]*x[i];
    }
  if(p == 0.0)
    b = 0.0;
  else
    {
      double a = pow(x[0]*x[0]+p, 0.5); //a为x的2范数
      if( x[0] <= 0)
	x[0] -= a;
      else
	x[0] = (-1)*p/(x[0]+a);
      b = 2*x[0]*x[0]/(p+x[0]*x[0]);
      num_vector_product(x, 1.0/x[0], l);
    }

  return b;
};

/////////////////////////
//计算两个向量的乘积,结果为一个矩阵
////////////////////////
void Vector_Product(double** A, double* x, double* y, int n)
{
  for(int i = 0; i < n; i++)
    {
      for(int j = 0; j < n; j++)
	{
	  A[i][j] = x[i]*y[j];
	}
    }
};

//////////////////
//用Householder变换计算A的QR分解
////////////////////
void QR_Resolution_Householder(double** A, double* y, double* d, int m, int n)
{
  for(int j = 0; j < n; j++) //对矩阵A的第j列求Householder变换
    {
      if(j < m)
	{
	  int l = m-j; //向量长度
	  double* v = new double[l]();
	  double b = Householder(A, v, j, m);
	  /*for(int i = 0; i < l; i++)
	    std::cout << v[i] << " ";
	    std::cout << std::endl;*/
	  d[j] = b;
	  double** I = new double*[l];
	  for(int i = 0; i < l; i++)
	    {
	      I[i] = new double[l]();
	    }
	  Vector_Product(I, v, v, l);
	  num_matrix_product(I, -b, l);
	  for(int i = 0; i < l; i++)
	    I[i][i] += 1;
	  double** tmp_A = new double*[l];
	  double* tmp_y = new double[l];
	  for(int i = 0; i < l; i++)
	    tmp_A[i] = new double[n-j];
	  for(int i = 0; i < l; i++) //行
	    {
	      double t = 0.0;
	      for(int k = 0; k < l; k++)
		{
		  t += I[i][k]*y[j+k];
		}
	      tmp_y[i] = t;
	      for(int k = 0; k < n-j; k++) //列
		{
		  t = 0.0;
		  for(int p = 0; p < l; p++)
		    {
		      t += I[i][p]*A[j+p][j+k];
		    }
		  tmp_A[i][k] = t;
		}
	    }
	  for(int i = 0; i < l; i++)
	    {
	      y[j+i] = tmp_y[i];
	      for(int k = 0; k < n-j; k++)
		A[j+i][j+k] = tmp_A[i][k];
	    }
	  /*for(int i = 0; i < m; i++)
	    {
	      for(int k = 0; k < n; k++)
		std::cout << A[i][k] << " ";
	      std::cout << std::endl;
	    }*/
	  for(int i = 1; i < l; i++)
	    A[j+i][j] = v[i];
	  /*for(int i = 0; i < m; i++)
	    {
	      for(int k = 0; k < n; k++)
		std::cout << A[i][k] << " ";
	      std::cout << std::endl;
	    }*/
	  for(int i = 0; i < l; i++)
	    {
	      delete [] I[i];
	      delete [] tmp_A[i];
	    }
	  delete [] tmp_y;
	  delete [] tmp_A;
	  delete [] I;
	  delete [] v;
	}
    }
};

//////////////////////////
//QR分解求解LS问题
/////////////////////////
void LS_QR_Householder(double** A, double* b, int m, int n)
{
  double* d = new double[n]();
  QR_Resolution_Householder(A, b, d, m, n);
  Upper_Triangular_Matrix(A, b, n);
  delete [] d;
};

////////////////////////////////
//返回向量x的2范数
////////////////////////////////
double Norm_Vector_2(double* x, int n)
{
  double norm = 0.0;
  for(int i =0; i < n; i++)
    norm += x[i]*x[i];
  norm = pow(norm, 0.5);
  return norm;
};

////////////////////////////////
//Jacobi迭代法
////////////////////////////////
void Jacobi_Iteration(double** A, double* b, double* x,  double condition, int n)
{
  int k = 1;//迭代次数
  double* y = new double[n]();
  double* z = new double[n]();
  for(int i = 0; i < n; i++)
    y[i] = x[i];
  do
    {
      for(int i = 0; i < n; i++)
	{
	  double tmp = 0.0;
	  for(int j = 0; j < n; j++)
	    {
	      if(j != i)
		tmp += -1.0/A[i][i]*A[i][j]*y[j];  
	    }
	  tmp += b[i]/A[i][i];
	  x[i] = tmp;
	}
      /*for(int i = 0; i < n; i++)
	std::cout << x[i] << " ";
	std::cout << std::endl;*/
      for(int i = 0; i < n; i++)
	z[i] = x[i] - y[i];
      if(Norm_Vector_2(z,n) <= condition)
	{
	  std::cout << Norm_Vector_2(z,n) << std::endl;
	  break;
	}
      else
	{
	  k++;
	  for(int i = 0; i < n; i++)
	    y[i] = x[i];
	}
    }while(k <= pow(10,7));
  std::cout << k << std::endl;
  for(int i = 0; i < n; i++)
    std::cout << "x[" << i+1 << "] = " << x[i] << std::endl;
  
  delete [] z;
  delete [] y;
};

//////////////////////////////
//Gauss-Seidel迭代
//////////////////////////////
void GS_Iteration(double** A, double* b, double* x, double condition, int n)
{
  int k = 1;//迭代次数
  double* y = new double[n]();
  double* z = new double[n]();
  for(int i = 0; i < n; i++)
    y[i] = x[i];
  do
    {
      for(int i = 0; i < n; i++)
	{
	  double tmp = 0.0;
	  for(int j = 0; j < i; j++)
	    {
	      tmp += -1.0/A[i][i]*A[i][j]*x[j];
	    }
	  for(int j = i+1; j < n; j++)
	    {
	      tmp += -1.0/A[i][i]*A[i][j]*y[j];
	    }
	  tmp += b[i]/A[i][i];
	  x[i] = tmp;
	}
      for(int i = 0; i < n; i++)
	z[i] = x[i] - y[i];
      if(Norm_Vector_2(z,n) <= condition)
	{
	  std::cout << Norm_Vector_2(z,n) << std::endl;
	  break;
	}
      else
	{
	  for(int i = 0; i < n; i++)
	    y[i] = x[i];
	  k++;
	}
    }while(k <= pow(10,7));
  std::cout << k << std::endl;
  for(int i = 0; i < n; i++)
    std::cout << "x[" << i+1 << "] = " << x[i] << std::endl;

  delete [] y;
  delete [] z;
};

//////////////////////////////////////
//SOR迭代
/////////////////////////////////////
void SOR(double** A, double* b, double* x, double w, double condition, int n)
{
  int k = 1;//迭代次数
  double* y = new double[n]();
  double* z = new double[n]();
  for(int i = 0; i < n; i++)
    y[i] = x[i];
  do
    {
      for(int i = 0; i < n; i++)
	{
	  double tmp = (1-w)*y[i] + w*b[i]/A[i][i];
	  for(int j = 0; j < i; j++)
	    tmp -= w*A[i][j]*x[j]/A[i][i];
	  for(int j = i+1; j < n; j++)
	    tmp -= w*A[i][j]*y[j]/A[i][i];
	  x[i] = tmp;
	}
      for(int i = 0; i < n; i++)
	z[i] = x[i] - y[i];
      if(Norm_Vector_2(z,n) <= condition)
        {
	  std::cout << Norm_Vector_2(z,n) << std::endl;
	  break;
	}
      else
	{
	  for(int i = 0; i < n; i++)
	    y[i] = x[i];
	  k++;
	}
    }while(k <= pow(10,7));
  std::cout << "Iteration speed of SOR with w = "
	    << w << ": " << k << std::endl;
  for(int i = 0; i < n; i++)
    std::cout << "x[" << i+1 << "] = " << x[i] << std::endl;
  
  delete [] y;
  delete [] z;
};

///////////////////////////////
//求两个R^n向量的差
//差储存在z中
///////////////////////////////
void Vector_Difference(double* x, double* y, double* z, int n)
{
  for(int i = 0; i < n; i++)
    z[i] = x[i] - y[i];
};

//////////////////////////////////////
//共轭梯度法
//////////////////////////////////////
void ConjugateGradient(double** A, double* b, double* x, int n)
{
  int k = 0;
  double* r1 = new double[n]();
  double* r2 = new double[n]();
  double* p1 = new double[n]();
  double* p2 = new double[n]();
  double* tmp = new double[n]();
  Matrix_Vector_Product(A, x, tmp, n);
  Vector_Difference(b, tmp, r1, n);
  delete [] r1;
  delete [] r2;
  delete [] p1;
  delete [] p2;
  delete [] tmp;
};

///////////////////////////////
//求两个R^n向量的差
//差储存在z中
///////////////////////////////
void Vector_Add(double* x, double* y, double* z, int n)
{
  for(int i = 0; i < n; i++)
    z[i] = x[i] + y[i];
};


/////////////////////////////////
//幂法求按模最大特征值
/////////////////////////////////
void EigenvaluePower(double** A,double condition, int n)
{
  double* x = new double[n]();
  double* y = new double[n]();
  double* z = new double[n]();
  for(int i = 0; i < n; i++)
    y[i] = x[i] = 1.0;
  int maxtime = pow(10,5); //maximum times of conjugation
  int k = 0;
  double max = 0.0;
  while(k < maxtime)
    {
      Matrix_Vector_Product(A, y, x, n);
      std::cout << "k = " << k << " ";
      for(int i = 0; i < n; i++)
	std::cout << x[i] << " ";
      std::cout << std::endl;
      max = Norms_Vector_Infinite(x,n);
      num_vector_product(x, 1.0/max, n);
      Vector_Difference(x, y, z, n);
      if(Norms_Vector_Infinite(z,n) < condition)
	{
	  std::cout << "v1>0" << std::endl;
	  for(int i = 0; i < n; i++)
	    y[i] = x[i];
	  break;
	}
      else
	{
	  Vector_Add(x, y, z, n);
	  if(Norms_Vector_Infinite(z,n) < condition)
	    {
	      std::cout << "v1<0" << std::endl;
	      max = -max;
	      for(int i = 0; i < n; i++)
		y[i] = x[i];
	      break;
	    }
	  else
	    {
	      for(int i = 0; i < n; i++)
		y[i] = x[i];
	      k++;
	    }
	}
    }
  std::cout << "Eigenvalue: " << max << " v = ";
  for(int i = 0; i < n; i++)
    std::cout << y[i] << " ";
  std::cout << std::endl;
  delete [] y;
  delete [] z;
};

////////////////////////////////////////
//Givens变换
////////////////////////////////////////
void Givens(double** A, int m, int k, double c,double s, int n)
{
  double t = 0.0;
  if(m >= 1){
    if(fabs(A[m][n]) > fabs(A[m-1][n])){
      t = A[m-1][n]/A[m][n];
      s = 1.0/pow((1+t*t), 0.5);
      c = s*t;
    }
    else{
      t = A[m][n]/A[m-1][n];
      c = 1.0/pow((1+t*t), 0.5);
      s = c*t;
    }
  }
  else
    std::cout << "m should be >= 1." << std::endl;
}

///////////////////////////////////////////
//用Householder变换完成上Hessenberg变换的其中一步
//k表示矩阵A的第k列，A为m*m矩阵
//对A中第k列下方长度为m-k的向量求householder变换
//求得v为第一个元素为1，长度为m-k的向量,储存在x中
///////////////////////////////////////////
double Hessenberg_Householder(double** A, double* x, int k, int m)
{
  int l = m-k-1; //向量x的长度
  for(int i = 0; i < l; i++)
    x[i] = A[k+i+1][k];
  num_vector_product(x, 1.0/Norms_Vector_Infinite(x, l), l);
  double p = 0.0; //向量x除去第一个元素的平方和
  double b = 0.0;
  for(int i = 1; i < l ; i++)
    {
      p += x[i]*x[i];
    }
  if(p == 0.0)
    b = 0.0;
  else
    {
      double a = pow(x[0]*x[0]+p, 0.5); //a为x的2范数
      if( x[0] <= 0)
	      x[0] -= a;
      else
	      x[0] = (-1)*p/(x[0]+a);
      b = 2*x[0]*x[0]/(p+x[0]*x[0]);
      num_vector_product(x, 1.0/x[0], l);
    }

  return b;
};

///////////////////////////////////////////
//H为n*n矩阵，A取右上角m*n与之相乘
//A*H
//A为r*r矩阵
//////////////////////////////////////////
void Matrix_Product(double** A, double ** H, int m, int r)
{
    int n = r-m;
    double** tmp = new double*[m];
    for(int i = 0; i < m; i++)
    {
        tmp[i] = new double[n]();
    }
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < n; j++)
        {
            double t = 0.0;
            for(int k = 0; k < n; k++)
            {
                t += A[i][m+k]*H[k][j];
            }
            tmp[i][j] = t;
        }
    }
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < n; j++)
        {
            A[i][m+j] = tmp[i][j];
        }
    }
    for(int i = 0; i < m; i++)
    {
        delete [] tmp[i];
    }
    delete [] tmp;
}

///////////////////////////////////////////
//H为n*n矩阵，A为r*r矩阵，A取m*n与之相乘
//A*H
//A分块矩阵的第一个元素坐标为[a,b]（从0开始计）
//////////////////////////////////////////
void Matrix_Product_Left(double** A, double ** H, int a, int b, int m, int n)
{
    double** tmp = new double*[m];
    for(int i = 0; i < m; i++)
    {
        tmp[i] = new double[n]();
    }
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < n; j++)
        {
            double t = 0.0;
            for(int k = 0; k < n; k++)
            {
                t += A[a+i][b+k]*H[k][j];
            }
            tmp[i][j] = t;
        }
    }
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < n; j++)
        {
            A[a+i][b+j] = tmp[i][j];
        }
    }
    for(int i = 0; i < m; i++)
    {
        delete [] tmp[i];
    }
    delete [] tmp;
}

///////////////////////////////////////////
//H为m*m矩阵，A为r*r矩阵，A取m*n与之相乘
//H*A
//A分块矩阵的第一个元素坐标为[a,b]（从0开始计）
//////////////////////////////////////////
void Matrix_Product_Right(double** A, double ** H, int a, int b, int m, int n)
{
    double** tmp = new double*[m];
    for(int i = 0; i < m; i++)
    {
        tmp[i] = new double[n]();
    }
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < n; j++)
        {
            double t = 0.0;
            for(int k = 0; k < m; k++)
            {
                t += H[i][k]*A[a+k][b+j];
            }
            tmp[i][j] = t;
        }
    }
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < n; j++)
        {
            A[a+i][b+j] = tmp[i][j];
        }
    }
    for(int i = 0; i < m; i++)
    {
        delete [] tmp[i];
    }
    delete [] tmp;
}

///////////////////////////////////////////
//H为m*m矩阵，A取左下角m*m与之相乘
//H*A*H
//A为n*n矩阵
//////////////////////////////////////////
void Matrix_ProductHAH(double** A, double ** H, int m, int n)
{
    double** B = new double*[m];
    for(int i = 0; i < m; i++)
        B[i] = new double[m]();
    // B = HA
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < m; j++)
        {
            double tmp = 0.0;
            for(int k = 0; k < m; k++)
            {
                tmp += H[i][k]*A[n-m+k][n-m+j];
            }
            B[i][j] = tmp;
        }
    }
    // A的左下角m*m矩阵 = BH = HAH
    for(int i = 0; i < m; i++)
    {
        for(int j = 0; j < m; j++)
        {
            double tmp = 0.0;
            for(int k = 0; k < m; k++)
            {
                tmp += B[i][k]*H[k][j];
            }
            A[n-m+i][n-m+j] = tmp;
        }
    } 
    for(int i = 0; i < m; i++)
        delete [] B[i];
    delete [] B;
}

//////////////////////////////////////////////
//上Hessenberg化
//////////////////////////////////////////////
void Hessenberg_Householder_Resolution(double** A, double** Q, int n)
{
  for(int j = 0; j < n-2; j++)
  {
    int l = n-j-1; //向量长度
    double* v = new double[l]();
    double b;
    b = Hessenberg_Householder(A, v, j, n);
    double** I = new double*[l];
    for(int i = 0; i < l; i++)
    {
      I[i] = new double[l]();
    }
    Vector_Product(I, v, v, l);
    num_matrix_product(I, -b, l);
    for(int i = 0; i < l; i++)
	    I[i][i] += 1;
  /*  if(j == 0)
    {
      for(int i = 0; i < n; i++)
      {
        for(int k = 0; k < n; k++)
          Q[i][k] = I[i][k];
      }
    }
    else
    {
      Matrix_Product_Left(Q, I, j+1, j+1, l, l);
    }*/
    Matrix_Product_Right(A, I, j+1, j, n-j-1, n-j);
    Matrix_Product_Left(A, I, 0, j+1, n, n-j-1);
    for(int i = 0; i < l; i++)
    {
	    delete [] I[i];
	}
    delete [] I;
    delete [] v;
  }
}

////////////////////////////////////
//双重步位移法,H为n*n矩阵
//对矩阵H对角线第m个元素开始的k*k矩阵应用双重步位移法
////////////////////////////////////
void Double_Shifted_QR(double** H, double** Q, int m, int k, int n)
{
    //将B赋值为所要迭代的k*k矩阵
    double** B = new double*[k];
    for(int i = 0; i < k; i++)
        B[i] = new double[k]();
    for(int i = 0; i < k; i++)
    {
        for(int j = 0; j < k; j++)
            B[i][j] = H[m-1+i][m-1+j];
    }
    double s = B[k-2][k-2] + B[k-1][k-1];
    double t = B[k-2][k-2]*B[k-1][k-1] - B[k-2][k-1]*B[k-1][k-2];
    double x = B[0][0]*B[0][0] + B[0][1]*B[1][0] - s*B[0][0] + t;
    double y = B[1][0]*(B[0][0] + B[1][1] - s);
    double z = B[1][0]*B[2][1];
    double* v1 = new double[3]();
    double* v = new double[3]();
    double** I = new double*[3];
    double b;
    for(int i = 0; i < 3; i++)
        I[i] = new double[3]();
    for(int i = 0; i < k-2; i++)
    {
        v1[0] = x;
        v1[1] = y;
        v1[2] = z;
        b = Householder(v1, v, 3);
        Vector_Product(I, v, v, 3);
        num_matrix_product(I, -b, 3);
        for(int i = 0; i < 3; i++)
	        I[i][i] += 1;
        Matrix_Product_Left(Q, I, 0, m-1+i, n, 3);
        int q = (i>1)?i:1;
        Matrix_Product_Right(B, I, i, q-1, 3, k-q+1);
        int r = (i+4<k)?(i+4):k;
        Matrix_Product_Left(B, I, 0, i, r, 3);
        x = B[i+1][i];
        y = B[i+2][i];
        if(i < k-3)
            z = B[i+3][i];
    }
    delete [] v;
    delete [] v1;
    for(int i = 0; i < 3; i++)
        delete [] I[i];
    delete [] I;
    double* v3 = new double[2]();
    double* v2 = new double[2]();
    v2[0] = x;
    v2[1] = y;
    double** I2 = new double*[2];
    for(int i = 0; i < 2; i++)
        I2[i] = new double[2]();
    b = Householder(v2, v3, 2);
    Vector_Product(I2, v3, v3, 2);
    num_matrix_product(I2, -b, 2);
    for(int i = 0; i < 2; i++)
	    I2[i][i] += 1;
    Matrix_Product_Left(Q, I2, 0, m+k-3, n, 2);
    Matrix_Product_Right(B, I2, k-2, k-3, 2, 3);
    Matrix_Product_Left(B, I2, 0, k-2, k, 2);
    for(int i = 0; i < 2; i++)
        delete [] I2[i];
    delete [] I2;
    delete [] v3;
    for(int i = 0; i < k; i++)
    {
        for(int j = 0; j < k; j++)
            H[m-1+i][m-1+j] = B[i][j]; 
    }
}

////////////////////////////////////
//隐式QR算法
////////////////////////////////////
void Implicit_QR(double** A, double** Q, double u, int n)
{
    Hessenberg_Householder_Resolution(A, Q, n);
    int m = 0;
    int l = 0;
    int k = 0;   
    do
    {
        for(int i = 1; i < n; i++)
        {
            if(fabs(A[i][i-1]) <= (fabs(A[i][i])+fabs(A[i-1][i-1])*u))
                A[i][i-1] = 0.0;
        }
        k = 0;
        for(int i = n-1-m; i > 0; i--)
        {
            if(A[i][i-1] == 0)
            {
                m++;
            }
            else
                break;
        }
        if(m == n)
            break;
        for(int i = n-1-m; i > 0; i--)
        {
            if(A[i][i-1] != 0)
                k++;
            else
                break;
        }
        l = n-m-k;
        Double_Shifted_QR(A, Q, l+1, k, n);
    } while (m!=n);
    for(int i = 0; i < n; i++)
    {
        std::cout << A[i][i] << " : ";
        for(int j = 0; j < n; j++)
        {
            std::cout << Q[i][j] << " ";
        }
        std::cout << std::endl;
    }            
}

double sigma1(double** A, int n)
{
  double s = 0.0;
  for(int i = 0; i < n; i++)
    for(int j = 0; j < n; j++)
      s += (j!=i)?(A[i][j]*A[i][j]):0;
  return s;
}

double Norm_Matrix_F(double** A, int n)
{
  double s = 0.0;
  for(int i = 0; i < n; i++)
    {
      for(int j = 0; j < n; j++)
	{
	  s += A[i][j]*A[i][j];
	}
    }
  return pow(s, 0.5);
}

void max(double** A, int* v, int n)
{
  for(int i = 0; i < n; i++)
    {
      for(int j = 0; j < n; j++)
	{
	  if(fabs(A[i][j]) > fabs(A[v[0]][v[1]]) and i!=j)
	    {
	      v[0] = i;
	      v[1] = j;
	    }
	}
    }
}

void Jacobi(double** A, double tol, int n)
{
  double** V = new double*[n]; //初始化特征矩阵
  for(int i = 0; i < n; i++)
    {
      V[i] = new double[n]();
      V[i][i] = 1.0;
    }
  double d = tol*Norm_Matrix_F(A,n); //可容许的误差
  int* v = new int[2];
  int p = v[0] = 0;
  int q = v[1] = 1; //绝对值最大的元素下标
  double* Ap = new double[n]();
  double* Aq = new double[n]();
  while(sigma1(A,n) > tol*Norm_Matrix_F(A,n))
    {
      max(A,v,n);
      p = v[0];
      q = v[1];
      double tau,c,s,t;
      if(A[p][q] != 0)
	{
	  tau = (A[q][q]-A[p][p])/(2*A[p][q]);
	  if(tau >= 0)
	    t = 1.0/(tau+pow((1+tau*tau),0.5));
	  else
	    t = 1.0/(tau-pow((1+tau*tau),0.5));
	  c = 1.0/(pow((1+t*t),0.5));
	  s = t/(pow((1+t*t),0.5));
	}
      else
	{
	  c = 1.0;
	  s = 0.0;
	}
      for(int i = 0; i < n; i++)
	{
	  if(i != p && i != q)
	    {
	       Ap[i] = c*A[p][i] - s*A[q][i];
	       Aq[i] = s*A[p][i] + c*A[q][i];
	    }
	  else if (i == p)
	    {
	      Ap[i] = A[p][p] - t*A[p][q];
	      Aq[i] = 0.0;
	    }
	  else
	    {
	      Ap[i] = 0.0;
	      Aq[i] = A[p][p] + t*A[p][q];
	    }
	}
      for(int i = 0; i < n; i++)
	{
	  A[i][p] = A[p][i] = Ap[i];
	  A[i][q] = A[q][i] = Aq[i];
	}
      for(int i = 0; i < n; i++)//p、q列第i个元素
	{
	  Ap[i] = V[i][p]*c - V[i][q]*s;
	  Aq[i] = V[i][p]*s + V[i][q]*c;
	}
      for(int i = 0; i < n; i++)
	{
	  V[i][p] = Ap[i];
	  V[i][q] = Aq[i];
	}	   
    }
  for(int i = 0; i < n; i++)
    {
      std::cout << A[i][i] << " ";
      /*      for(int j = 0; j < n; j++)
	std::cout << V[i][j] << " ";
	std::cout << std::endl;*/
    }
  for(int i = 0; i < n; i++)
    delete [] V[i];
  delete [] V;
  delete [] Ap;
  delete [] Aq;
}
